//
// Created by jimmyteng on 19-4-10.
//
#include <opencv2/opencv.hpp>
#include "visual_compass/visual_compass.h"

cv::Mat rotateImage(cv::Mat img, double degree,double scale = 1.0) {
  double angle = degree * CV_PI / 180.; // 弧度
  double a = sin(angle), b = cos(angle);
  int width = img.cols;
  int height = img.rows;
  int width_rotate = (height * fabs(a) + width * fabs(b))* scale;
  int height_rotate = (width * fabs(a) + height * fabs(b))* scale;

// 旋转中心
  cv::Mat map_matrix = cv::Mat(2, 3, CV_64F);
  cv::Point2f center = cv::Point2f(width / 2, height / 2);

  map_matrix = cv::getRotationMatrix2D(center, degree, scale);
  std::cout << map_matrix.type() << std::endl;
  map_matrix.at<double>(0, 2) = map_matrix.at<double>(0, 2) + (width_rotate - width) / 2;
  map_matrix.at<double>(1, 2) = map_matrix.at<double>(1, 2) + (height_rotate - height) / 2;
  cv::Mat img_rotate;
  warpAffine(img, img_rotate, map_matrix, cv::Size(width_rotate , height_rotate ), 1, 0, 0);
  return img_rotate;

}

int main() {
  int index = 500;
  while (true) {
    //read image
    std::string path = "/home/jimmyteng/img/img1";//set Data path
    auto filename = path + std::to_string(index) + ".png";
//    filename = "../resource/testdata/1.png";
    std::cout << filename << std::endl;
    cv::Mat image = cv::imread(filename, cv::IMREAD_GRAYSCALE);
    if (image.empty()) break;
    //Initialize parameter of camera

//    float camera_matrix[9] = {2.77401855e+02, 0., 3.20706146e+02,
//                              0., 2.77464752e+02, 2.48626984e+02,
//                              0., 0., 1.};
//    float distortion_coefficients[4] = {0.00372105, 0.0101423, -0.0117764, 0.00403357};
//
//    float r[9]{-9.97167826e-03,  9.99949038e-01, -1.59143505e-03,
//               -9.99057889e-01, -1.00299902e-02, -4.22225781e-02,
//               -4.22363840e-02,  1.16890576e-03,  9.99106944e-01 };

    float camera_matrix[9] = {2.77401855e+02, 0., 3.20706146e+02,
                              0., 2.77464752e+02, 2.48626984e+02,
                              0., 0., 1.};
    float distortion_coefficients[4] = {0.00372105, 0.0101423, -0.0117764, 0.00403357};

    float r[9]{-0.0198525, 0.99976, -0.00928507,
               -0.999449, -0.0195975, 0.0267938,
               0.0266054, 0.00981188, 0.999598};



//    float r[9]{-8.08247458e-03, 9.99967098e-01, 6.55866519e-04,
//               -9.99333203e-01, -8.05399381e-03, -3.56122330e-02,
//               -3.56057808e-02, -9.43264225e-04, 9.99365449e-01
//    };
//
//    float camera_matrix[9] = {2.79109619e+02, 0., 3.16118042e+02,
//                              0., 2.79484833e+02, 2.37545059e+02,
//                              0., 0., 1.};
//    float distortion_coefficients[4] = {-7.02879429e-02, 2.03178778e-01, -2.42307484e-01,
//                                        1.02940999e-01};

    cv::Mat K(3, 3, CV_32F, camera_matrix);
    cv::Mat DistCoef(1, 4, CV_32F, distortion_coefficients);
    cv::Mat R(3, 3, CV_32F, r);
    //Initialize
    VisualCompass::GetInstance()->Initialize(K, DistCoef, R);

    double during = 0, t = (double) cv::getTickCount();
    double res, score;
    VisualCompass::GetInstance()->Detect(image, res, score);
    during = (double) cv::getTickCount() - t;
    printf("execution time = %gms\n", during * 1000. / cv::getTickFrequency());

    auto resimage = rotateImage(image, static_cast<int>(res));
    std::cout << res << std::endl;
    cv::Vec3b p;
    p[0] = 0;
    p[1] = 255;
    p[2] = 0;
    image.at<cv::Vec3b>(240, 320) = p;
    image.at<cv::Vec3b>(239, 319) = p;
    image.at<cv::Vec3b>(239, 320) = p;
    image.at<cv::Vec3b>(240, 319) = p;
    cv::imshow("show", rotateImage(VisualCompass::GetInstance()->GetBinaryImage(), res));
    cv::imshow("undistorted", image);

    cv::imshow("res", rotateImage(image, res));
    cv::waitKey();
    ++index;
  }
  return 0;
}